#include <iostream>
#include "Eigen/Dense"
#include <Eigen/Core>
#include <cmath>
#include <iomanip>
#include<vector>
#include <numeric>
#include <stdlib.h>
#include <stdio.h>
using namespace Eigen;
using Eigen::MatrixXd;
using namespace std;
#define PI acos(-1)
// #include"swap.h"
#include "sisl.h"
#include<fstream>

class TrajPlanning
{
private:
	double velocity = 5, interplorationTime = 0.02;
	
public:
	int state_hold;
	vector<double> moveDirection_ver;
	
	vector<vector<double>> jointSpacePlan(vector<vector<double>>& anglePosition, vector<double>&time);
	vector<vector<double>> catesianSpacePlan(vector<vector<double>>& position, vector<vector<double>>& posture);
	Eigen::Matrix<double, 4, 4> RotaToHomo(Eigen::Matrix<double, 3, 3> Rota, Eigen::Matrix<double, 3, 1> Tran);
	vector<double> catesianSpacePlanOnLine(vector<double>& positionposture,double state);
};
